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SUMMARY 


The  proposed  research  is  divided  into  two  phases.  The  first  intorduces  the  PUTD 
( Pseudo- Uptriangular- Decomposition)  to  reduce  the  governing  equations  of  motion  of  artic¬ 
ulated  mechanical  systems.  Performance  of  such  systems  is  based  greatly  on  the  assumptions 
and  models  v.«ed  generate  *h~  proper  control  algorithms.  This  investigation  proposes  a 
new  method,  which  allows  the  constrained  systems  to  operate  in  the  presence  of  singularities. 
This  is  achieved  by  a  regularization  technique  which  makes  use  of  a  new  representation  of 
the  kinematical  and  geometrical  constraint  equations  at  singular  positions.  This  method  of 
stability  analysis  is  compared  with  the  asymtotic  stability  presented  by  Baumgarte.  The 
PUTD  is  extended  to  accommodate  the  dynamics  of  such  systems.  An  illustration  of  the 
utility  and  effectiveness  of  the  method  proposed  is  shown  through  a  two  arm  planar  robot 
undergoing  large  motions  and  driven  through  singularities.  The  driving  torques  are  then 
compared  to  check  for  discontinuities  and  jerks. 

The  results  show  clearly  that  without  the  regularization  and  stability  of  the  dynamics 
of  the  system  through  the  proposed  method,  large  peaks  for  driving  forces  and  discontinu¬ 
ities  of  the  velocities  and  accelerations  are  attained.  The  latter  could  hamper  seriously  the 
performance  and  the  mission  of  the  system. 

The  second  phase  of  the  research  project  set  the  stage  for  the  testing  of  the  proposed 
method  when  the  articulated  structures  are  composed  of  flexible  bodies.  The  complete 
matrix  formulation  of  the  equations  of  motion  is  presented  based  on  the  finite  element, 
modal  analysis.  Kane's  equations  and  the  PUTD  method.  Exploitation  of  the  pipelining 
feature  of  the  IBM  3090,c:  vector  processor  by  implementing  the  code  on  this  machine  and 
subjecting  it  to  suitable  vectorization  in  the  computationally  intensive  areas.  This  cuts 
down  the  CPU  time  drastically  needed  for  the  dynamic  simulation  of  multibody  dynamical 
systems. 
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2.  Introduction 


The  handling  of  the  constraints  in  multibody  dynamics  could  be  done  in  two  different 
ways.  First  by  introducing  the  so-called  Lagrange  undetermined  multipliers  then  the  dynam¬ 
ics  of  the  system  is  found  by  solving  the  equations  of  motion  together  with  the  constraint 
equations  which  are  expressed  at  the  acceleration  level.  This  approach  leads  to  solving  more 
equations  than  needed,  hence  it  is  computationally  expensive.  The  second  alternative  is  to 
reduce  the  governing  equations  by  eliminating  the  undetermined  multipliers  through  pre¬ 
multiplication  by  a  matrix,  orthogonal  complement  to  the  constraint  Jacobian  matrix.  This 
approach  is  best  suited  for  the  case  when  the  constraint  forces  are  no  object  in  the  analysis. 

At  special  configurations,  the  constraint  Jacobian  matrix  may  become  less  than  full  rank, 
hence  serious  difficulties  arise  in  extracting  the  orthogonal  complement  array  and  inverting 
the  generalized  mass  matrix.  Therefore  at  such  instances,  the  constraint  equations  need  to 
be  modified  in  such  a  way  to  avoid  singularities  in  the  numerical  method  of  solution.  We 
propose  a  new  representation  of  the  constraints  when  this  situation  occur.  These  modified 
equations  should  be  valid  at  the  neighborhood  of  the  special  configuration  where  the  Jacobian 
matrix  changes  its  rank,  and  therefore  they  are  useful  only  if  the  system  is  in  motion  when 
the  special  configuration  occurs.  In  addition,  to  avoid  the  accumulation  of  the  numerical 
errors  in  integration.  Baumgarte's  method  Tj  of  numerical  stabilization  is  also  included. 

The  systematic  reduction  of  the  equations  of  motion  could  be  achieved  through  several 
approaches  i  1-6 j .  Those  methods  while  their  computational  schemes  differ,  their  objective 
is  to  extract  the  orthogonal  complement  array  to  the  Jacobian  constraint  matrix.  In  this 
paper  we  propose  to  address  the  issue  of  numerical  stability  resulting  from  the  constraints 
when  they  become  less  than  full  rank.  The  intention  of  this  research  is  to  highlight  and 
further  demonstrate  the  utility  of  the  method  proposed  through  a  comprehensive  analysis  of 
a  planar  robot  in  motion  passing  through  a  singular  position. 

This  report  is  divided  into  several  sections.  The  first  is  summary.  The  second  section  is 
the  Introduction.  The  development  of  the  equations  of  motion  for  constrained  mechanical 
systems  and  Baumgarte's  stability  method  form  the  subsections  one  and  two  of  section  3. 
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In  section  four  we  will  present  a  new  method  for  the  stabilization  problem  when  some  rows 
of  the  constraint  Jacobian  matrix  vanish  at  the  neighborhood  of  singular  position,  and  when 
some  of  them  become  linearly  dependent.  An  illustrative  example  is  given  in  section  4.  The 
stability  of  the  flexible  systems  is  discussed  in  section  5.  Section  6  forms  the  conclusions  and 
the  future  directions. 


3.  Theoritical  Development 

3.1  Equations  Formulations  for  Constrained  Systems 

The  governing  e  [uations  of  motion  of  a  nmltibody  system  could  be  obtained  using  Kane  s 
equations 


/*  +  /;=  0  /=  1,2,. ..,6^ 


where  fi  is  the  generalized  active  force  array  and  is  given  by 


(1) 


ft  =  I  hFk  -  (‘2) 

whereas  /*  defines  the  generalized  inertia  force  array 

/;  =  vkF;  -  O) 

in  both  equations  (2)  and  (3)  V*.  and  u,'fe  are  the  corresponding  partial  velocity  (see  Amirouche 
and  Jyia.  1987)  associated  with  the  mass  center  velocity  and  angular  velocity  of  B^  in  a 
reference  frame  R.  In  equation  (2)  F^  and  Tk  are  the  h,„n  components  of  the  vector  force  F \ 
and  the  torque  Tf.  (Note  that  a  force  acting  on  a  body  B *.  could  be  replaced  by  a  force  and 
moment  acting  at  the  mass  center  of  Bk ) 

In  equation  (3).  however,  the  Ffe*  and  Tk  are  the  firm  components  of  the  inertia  forces  iq* 
and  T* ,  where 
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Fk  ~  -m-ko-k 


(4) 


and 

Tk  =  -/fc-Qfc  -  ujkx(ik-uk)  (5) 

In  equation  (4)  mk  denotes  the  mass  of  body  Bk ,  and  ak  is  its  mass  center  acceleration. 
Ik  is  the  inertia  dyadic  of  Bk  relative  to  its  center  mass  and  expressed  with  respect  to  the 
Tinm  components  in  R\  u>k  is  the  angular  velocity  of  Bk  and  a*  its  corresponding  angular 
acceleration. 

If  a  multibody  system  is  subject  to  some  constraints  which  might  result  from  mechanical 
joints,  closed  loops  or  prescribed  motions,  then  equation  (1)  becomes 

+  K  +  =  0  /  =  1,2 . 6JV  (6) 

where  A,  is  the  so-called  Lagrange  multipliers  and  Bn  is  the  transpose  of  the  constraint 
Jacobian  matrix  (see  Huston  and  Wang. 1986).  Equation  (6)  is  simply 

ft  +  //  -  fi  =  0  /  =  1.2 . 6.V  (7) 

where  f['  are  the  generalized  constraint  forces. 

The  constraint  equations  could  be  expressed  at  the  velocity  level  as 


By  —  G  (7. a) 

where  G  is  a  function  of  time,  y  defines  the  generalized  speeds.  Further  differentiation  of 
eq(7.a)  yields 


By  a-  By  =  G  (7.6) 

In  the  dynamic  simulation  of  multibody  systems,  equation  (7.b)  is  most  useful  since 
it  forms  a  set  of  m  differential  equations  which  can  be  combined  with  equation  (6).  To 
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bring  the  equations  of  motion  to  a  minimum  dimension,  we  premultiply  equation  (6)  by  the 
orthogonal  complement  to  matrix  B.  This  results  in  an  n-m  equations,  which  together  with 
the  m  equations  given  by  equation  (7.b)  yields  the  governing  equations  of  motion. 

Orthogonal  Complement  Matrix.  If  there  are  m  constraint  equations  in  a  mechanical 
system  which  has  n  generalized  coordinates,  then  the  constraint  Jacobian  matrix  B  will  be 
an  m  x  n  matrix.  Consider  the  transpose  of  the  constraint  matrix  denoted  as  BT ,  it  is 
known  that  an  n  x  m  matrix  BT  can  be  reduced  to  an  upper  triangular  form,  say  U,  either 
by  Gauss-eliuuuaiion  row  operations  [12]  or  by  Pseudo-  uptriangular  decomposition  method 
21.  BT  and  U  are  row  equivalent  with  same  rank  and  the  row  equivalence  transformation 
can  be  expressed  as 


PBT  =  U  (8) 

where  P  is  an  n  x  n  nonsingular  matrix  obtained  by  applying  the  same  row’  operations  which 
transform  BT  to  U . 

Let  Ii  and  I2  denote  matrices  composed  of  respectively,  the  first  m  columns,  and  the  iast 
(n  —  m)  columns  of  the  nth  order  identity  matrix,  therefore.  I2  is  orthogonal  to  7X.  Since 
the  columns  of  L  are  a  linear  combination  of  the  columns  of  I\ ,  /2  is  also  orthogonal  to  V . 
Then,  using  equation(8).  we  obtain 


I/U  =  l/PBT  =  0  (9) 

or 

I2T  PBT  —  CBt  —  0  (10) 

where  C  —  Ij P  is  a  ( n  —  m )  x  n  matrix.  Since  P  is  nonsingular,  the  n  —  m  rows  of  C  are 
linearly  independent.  Therefore  in  Rn  dimensional  space,  the  n  —  m  rows  of  C  form  a  basis 
of  vectors  orthogonal  to  the  m  columns  of  the  transpose  of  constraint  matrix,  i.e.  C  is  a 
complement  matrix  to  BT . 
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Premultipling  equation  (6)  by  C  and  applying  equation  (10),  a  reduced  form  of  the 
equations  of  motion  of  a  constrained  multibody  system  is  obtained  as 

kp  +  k;  =  o  (id 

where 

Kp  =  C’fi  K;  =  Cf:  (12) 

in  equation  (11),  we  have  n  —  m  reduced  equations.  In  order  to  solve  for  the  dynamics  of  the 
system,  we  must  combine  the  m  constraint  equations  at  their  acceleration  level  with  those 
of  equation  (11)  to  obtain  the  time  history  response  of  the  system.  More  details  are  given 
in  next  section. 


3.2  Baumgarte  Stability  Method 

in  oraer  to  solve  the  governing  equation*,  we  usually  represent  the  constraints  at  their 
acceleration  level.  For  instance,  if  we  have  m  constraint  equations  (holonomic).  they  could 
be  represented  by 


M<?i*<?2,  •  •  •  qu,t)  =  0  i  =  1.2---m  (131 

the  q  s  are  the  generalized  coordinates  and  t  denotes  time.  Equation  (13)  is  said  to  be  a 
representation  of  the  constraints  at  the  position  level.  Further  differentiation  will  lead  to  its 
representation  at  the  velocity  level,  where 


,  " dhidq i  dhi  " 

=  aT  - b"yi 


9i  =  0 


14) 


z=i 


where  b u  is  the  (i.l)  element  of  the  m  x  n  constraint  matrix  B  defined  by 
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Oh, 

Oh, 

Oh i 

Oqx 

dqi 

Oqn 

Oh2 

0h2 

Oh 2 

Oqx 

Oq2 

Oqn 

Oh„ 

Ohm 

Ohm 

Oq i 

Oq2 

Oqn 

yi  is  the  Ith  element  of  the  matrix  Y  defined  by 


Y  = 


'hi 

dt 


and  gx  is  the  it>l  element  of  the  m  x 


T 


'hi  h* 

at  dt  ’ 

1  matrix  G  given  by 


(15) 


(16) 


G  = 


a/n 

at 


Qhi 

ot 


Ohr n 
Ot 


J 


(17) 


If  we  further  differentiate  equation!  14).  we  can  get  the  acceleration  representation  of  the 
constraints  as 


hi  =  ]T  6aj/j  -  y,  =  U  (18) 

(=i  /=i 

one  thing  we  should  keep  in  mind  is  that  hx  —  0  implies  ht  =  0.  hx  =  0.  but  /? ,  =  0  (which 
is  what  we  use  in  equation  (18))  does  not  necessarily  yield  hx  —  0  and  /i,  =  U.  Therefore 
it  is  important  to  know  that  during  the  integration  of  the  equations  of  motion  we  must 
try  to  satisfy  all  constraint  conditions.  In  the  event  we  don't.  It  is  possible  to  accumulate 
numerical  errors  which  might  cause  some  serious  stability  and  control  problems.  To  overcome 
this  difficulty,  Baumgarte  introduced  a  method  to  assure  asymptotic  stability  by  replacing 
hx  =  0  with 


hx  -f  ot{hx  —  3xhi  =  U  i  =  1 . 2.  •  •  •  m  ( 19 ) 

where  3X  =  0  for  nonholonomic  constraints,  a,  and  3X  arbitrary  constants  chosen  to  suffi¬ 
ciently  fast  decay  of  the  errors  (Baumgarte.  1972).  For  example,  let  us  consider  th~  constraint 
equations  hx  =  0  where  hi  =  0  is  a  holonomic  constraint.  In  addition,  assume  that,  in  the 
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process  of  the  integration  of  the  equations  of  motion  after  the  nth  step,  the  computer  yields 
the  values  h  =  8  h  —  t  which  deviate  from  the  exact  values  h  =  0.  h  =  0.  According  to 
the  differential  equation  h  =  0.  the  computer  should  produce  the  value  h  =  it  —  8.  Thus  the 
holonomic  constraint  is  not  satisfied  in  a  linearly  stable  taslnon.  It  is  unlikely  that  subsequent 
numerical  errors  will  compensate  for  this  behavior. 

By  virtue  of  the  initial  conditions  h{ 0)  =  0.  h( 0)  =  0.  equation!  191  leads  to  h  =  0 
as  before,  such  that  the  new  constraint  is  analytically  equivalent  to  h  =  0.  But  from  the 
numerical  point  of  view,  the  situation  is  different.  In  order  to  achieve  asymptotical  stability. 

ot  >  0  I  20 ) 

and 


o'  -  4d,il0  (21) 

must  be  satisfied.  We  usually  choose  Bamngarte.  1972 

«:  =  u  (22) 

Note  that  from  equation  120-21  I.  we  have  an  infinite  set  of  a  s  and  A  s  which  could  be  used 
in  equation  l  19). 


3.3  Proposed  Stability  in  the  Presence  of  Singularities 
a)  Regularization  of  Vanishing  Constraints 

It  is  common  m  the  course  of  a  mechanical  systems  motion  that  for  B.i  .  I  -  1  the 

Stk  constraint  equation  coefficients  may  become  all  zero  at  some  instantaneous  special  con¬ 
figurations.  In  this  case,  the  procedures  we  mentioned  m  previous  section  fail  in  determining 
the  orthogonal  complement  array  because  the  rank  of  B  becomes  less  than  m.  In  addition 
the  augmented  system  inertial  matrix  becomes  singular. 
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Since  the  generalized  constraint  forces  due  to  the  5th  constraint.  A ,B,i  /  =  l.---n. 
are  zero,  the  constraint  do  not  have  any  effect  on  the  system  at  that  configuration  and 
one  possible  procedure  is  deleting  the  Sth  row  of  B.  finding  the  corresponding  orthogonal 
complement  matrix  C\  and  then  proceeding  with  the  normal  coordinate  reduction  technique. 
As  we  mentioned  before  the  constraint  at  the  acceleration  level  could  be  expressed  as 

h,  =  Bay i  -  Bltyi  -  gx  =  0  i  =  I .  •  •  •  m  (23  i 

where  y  denotes  the  generalized  speeds.  Suppose  for  the  i  =  s.  Bft  —  0.  then  we  can  write 

h,  =  B,iyt  -  g„  =  0  i  24  ) 

Equation! 24)  must  be  satisfied  by  selecting  the  initial  conditions  for  yi  such  that 

Bain  =  9 ,  (  25  ) 

in  order  to  achieve  the  consistency  of  the  equations. 

When  the  Srh  const  raint  equation  is  removed  in  this  manner,  the  resulting  equations  can 
be  used  in  the  neighborhood  of  the  special  configuration.  The  drawback  of  this  approach  i? 
that  the  assumption  of  zero  constraint  forces  in  the  neighborhood  results  in  fast  deviation 
of  the  simulation  from  the  constrained  behavior. 

To  capture  the  effects  of  the  actual  constraint  forces  in  the  neighborhood  of  the  vanishing 
constraint,  we  differentiate  further  the  S'1'  constraint  equation.  This  will  provide  further 
information  on  the  constraint,  hence  a  modified  form  of  the  constraint  equation  will  be  used 
instead  of  its  deletion.  To  illustrate  this  approach,  consider  the  time  derivative  of  equation 
(23).  when  i  —  s 


h,  =  B,tyi  ~  2 Bay i  -  B,iyi  -  j,  =  <"*  !  -  l. ...  n 
To  simplify  the  representation  of  equation!  2fi).  we  express  B.(  as 
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D  dBtl  .  dB,t 

-  “3 - 9h  t - 3T- 

aqi,  at 


Since 


%  =  Thjyj  (28) 

where  T  is  a  matrix  relating  q  to  y.  In  the  light  of  the  above,  we  can  write  equation(27)  as 


B,i  —  OsijVj  -r 


9>slj  —  r,  -tkj 


n\,i  = 


Similarly,  g ,,  can  be  expressed  as 


g ..  =  ti.-iyi  - 


where 


_  %T 

M.W  O  Hlj 


_  %- 
r’  “ 


Further  differentiate  of  equation  (29)  and  (32)  gives  the  B  and  g  terms  as 


B tl  ^  ’.it 


9*  =  9>iyi  -  M*iyt  -  T' 


Substituting  equations  (29),  (34)  and  (35)  into  equation  (26).  we  get 
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Baiiii  4-  G,iyi  +  J„  —  0 


l  =  1,-  ■  •  ,n 


(36) 


where 


G,i  —  2B,i  -r  <t>„piyp  —  y,i  1-iP  —  (37) 

and 

J *  -  <t>,iPyPyi  +  i'siyi  -  y*m  -  (38) 

The  usefulness  of  equation  (36)  stems  from  the  assumption  that,  when  the  elements 
of  the  constraint  are  zero  at  the  special  configuration  and  small  at  its 

neighborhood,  is  negligible  compared  to  the  other  terms  in  equation) 36),  so  dropping 

that  term  in  the  equation  will  yield 


K  =  Gain  -  J,  =  0  (39) 

It  is  seen  that  and  J,  are,  in  general,  functions  of  qi,  yi  and  t.  Equation) 39)  is  lint  ■r 
in  accelerations  yt  and  has  the  form  of  the  constraint  at  the  acceleration  level.  If  one  ; 
interested  in  computing  the  generalized  constraint  forces  at  the  neighborhood  of  singularity, 
then  all  we  have  to  do  is  replace  in  Fr  =  \BT .  the  Sth  constraint  by 

B,t  =  G.i  (40) 

In  most  cases.  G,i  doesn't  vanish  while  B„i  does.  The  governing  equations  of  motion  will 
then  be  subject  to  the  constraints  given  by  equation  (39). 

To  further  assure  the  numerical  stability  of  the  constraints,  we  can  make  use  of  Baumgarte 
technique  where  equation  (39)  takes  on  the  following  form 

G,iyi  -  J,  -  a  ,(  B,iyi  -  g,)  -  3,(  Bflyi  -  g,)  =  0  (41) 

which  reduces  to 
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G,tyi  +  J*  +  a,(B,iyt  -  g3)  -  3,gs  =  0 


(42) 


where 


G,tyi  =  -J,  -  a,(Bsiyi  -  g.)  +  3*9,  (43) 

and 

S,  =  -J,  -  Q,( B.'iVi  -  9,)- r  3*g*  (44) 

Note  that  h,  —  0  is  automatically  satisfied  since  =  0  ,  /  =  1 . n. 


b)  Regularization  of  Linearly  Dependent  Constraints 

In  a  similar  fashion  to  the  case  of  vanishing  constraints,  the  linear  dependency  causes  B 
to  be  less  than  full  rank.  In  this  case,  it  is  known  that  an  n  x  m  matrix  BT  can  be  reduced 

to  Gaussian  form  U  by  Gauss  elimination  row  operations,  where  the  rows  1 . r  are  the 

nonzero  rows  of  U’ .  and  the  leftmost  nonzero  entry  of  row  i  occurs  in  column  kx.  i  —  l. ...  .r 
then  k\  <  k2  <■  •  ■  <  kT. 

Bt  and  G*  are  row  equivalent  with  same  rank  r  and  the  row  equivalence  transformation 
can  be  expressed  as 


PBt  =  r*  (45) 

where  P  is  the  corresponding  n  x  n  nonsingular  matrix. 

It  is  seen  from  the  definition  of  U *  that  the  columns  with  indices  k,.  f  =  1 ..... r  are  lin¬ 
early  independent  columns,  whereas  the  remaining  m—r  columns  whose  indices  are  denoted 
as  dx,  i  =  1...  .,m  —  7- ,  can  be  expressed  as  linear  combinations  of  the  former  set.  The  d\h 
columns  of  BT  can  also  be  written  as  linear  combinations  of  the  k\h  columns  of  BT  To  show 
this,  we  can  write  equation(45)  as 
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Pb>  =  uj 


(46) 


where  b3  and  uJ  denote  the  jth  columns  of  BT  and  U*,  respectively.  For  the  columns  of  U *, 
we  have 


ud 1  =  Zj{Uk'  i  =  1,. . .  ,r  j  =  l, ...  ,m  —  r  (47) 

where  Z:i,i  =  1, .  . .  ,r  are  the  linear  combination  constants  for  the  dtk  column.  Substitution 
of  equation  (46)  into  equation  (47)  for  ud>  and  for  each  uk'  leads  to 

Pbd>  =  ud>  =  ZjiUk'  =  ZjtPbk'  (48) 

so. 

bd}  =  Zjibk'  i  =  1,. . .  ,r  j  =  1, ...  ,m  —  r  (49) 

representing  equation  (49)  in  a  matrix  form,  we  obtain 


Bd,i  —  ZjzBk,i  —  0  (50) 

we  can  write  the  constraint  equations  in  these  following  forms 

hd,  =  Bdjly  -  gdj  =  0  (51) 

hk,  =  Bk,iV  ~  9k,  =  0  (52) 

hd,  =  Bdjiy  -+■  Bdiiy  -  gdj  =  0  (53) 

bk,  =  Sfc.iy  --  Bktiy  -  gk.  =0  (54) 


we  can  use  equation  (54)  to  generate  our  constraint  equations  at  the  acceleration  level, 
but  we  can  t  use  equation  (53)  because  Bd]i  is  linearly  dependent  to  Bkj,  as  shown  in  equation 
(50).  Since 
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hd,  ~  Zjihk,  =  { Bd]i  -  ZjiBk,]y  -  [gdj  -  Zjigk<}  =  0 


(55) 


and  using  equation  (50),  equation  (55)  becomes 

~  Zjihk<  =  -{gdj  -  ZJtgkt\  =0  (56) 

its  differentiated  form  yields 

hd,  ~  Z]thkt  =  (Bd]i  -  Z:iBkt)y  +  (Bd,i  ~  ZoiBkJ)y  -  (gdj  -  Zjtgk< )  =  0  (57) 

making  use  of  equation  (50)  once  more,  equation  (57)  reduces  to 

hd,  -  Z^h^  =  ( Bd]i  -  ZjiBkti)y  -  {gdj  -  Z]xgkx)  =  0  (58) 

Note  how  the  y  term  is  not  explicit  in  equation  (58).  If  we  further  differentiate  equation(57), 
we  get 


ill  _  in  _ 

hd,-Zjihkt  =  (B<iii-ZjiBkii)y  +  2{Bdjl-ZjiBkli)y  +  (Bdji-ZjiBkil)y-(gdj-Zjigkt)  =  0  (59) 


equation  (59)  reduces  further  to 


hd}  ~  ZJthki  -  2 (Bd}i  -  Z]XBkxi)y  ( Bdd  -  Z:xBktl)y  ~  (gdj  -  Zjt'gkl  >  -  0  (60) 

by  dropping  the  term  that  multiplies  y  by  virtue  of  equation  (50).  The  representation  of  the 
constraint  equations  given  by  equation  (53)  are  now  represented  by  equation  (60).  as 

h’d,  =  2 (Bdjt  -  ZJtBkti)y  +  ( Bdjl  -  Z:iBkJ)y  -  ( gd>  -  ZJtgkl )  =  0  (61) 

equation  (61)  and  equation  (54)  are  linearly  independent  and  form  a  consistent  set  of  con¬ 
straint  equations  to  be  used  when  the  linear  dependency  of  the  constraints  occurs.  This 
stability  procedure  could  be  further  enhanced  by  applying  Baumgarte's  method  to  stabilize 
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the  numerical  error  due  to  the  integration  of  the  equations  of  motion.  Employing  Baumgarte 
technique  we  further  obtain  a  form  of  the  constraint  as 


2 {Bd]i  -  ZjiBkxi)y  +  ( Bdjl  -  Z^B^y  -  ( gdj  -  Zji'gk,) 

+adj[{Bdii  -  Zji Bk,i)y  -  ( gd ,  -  Zjtgki)\  +  f3d][-(gdj  -  ZJtgkJ]  =  0  (62) 

writing  the  above  equation  in  a  compact  form  we  get 

Gd,y  +  Sd}=  0  (63) 

where 

Gd]  =2(Bdjl-ZjiBk,l)  (64) 

and 

$dj  (9dj  ^jiQk ,)  y[{Bd,l  &<ij  Bdji  )  Zji(Bk%l  “t"  Q-d]Bkxl) 

-(ocd}gdi  -  3dj gdj )  -  Zji(adjgk,  -r  3djgkt )  (65) 

4.  Procedure  Verification  and  Applications 

4.1  Two-Arm  Robot  with  Specified  Motion 

Consider  a  two-bar  linkage,  Li  —  l,  L2  =  m2  =  m.  =  2m2  =  2 m  .  as  shown 
in  figure  1.  Assume  that  point  p  follows  the  horizontal  line  y  =  (  with  a  constant  velocity 
along  the  -x  axis.  We  know  that  for  qi  =0.  q2  =  tt,  the  system  becomes  singular.  This 
is  a  perfect  example  to  check  our  stability  method  and  further  compare  it  to  Baumgarte 
technique.  In  the  sequel  we  will  show  howr  our  stability  is  completely  independent  of  the 
asymptotic  stability  due  to  the  ill  representation  of  the  constraints  at  the  acceleration  level. 
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The  constraint  equations  for  the  problem  given  are 


M<7i>?2,0  =  Isinqi  -f  -sin(qx  +  q2)  -  -  =  0 


(66) 


h2{qi,q2,t)  =  /co$9i  +  ~co.s(9i  +  <?2)  4-  vt  -  x0  =  0  (67) 

where  x0  denotes  the  initial  position  of  the  x  coordinate.  Their  first  order  differentiated 
forms  yield 


h  1  —  ICiyi  -r  ~Ci2{y  1  f-  J/2)  —  0 


(68) 


h2  =  -ISiy!  ~  ^Su(yi  +  2/2 )  +  v  =  0  (69) 

where  Si  =  singx,  S12  =  5in(q1  +  <?2),  C*i  =  cosqi,  C\2  =  co-sf^  -f  q2),  3/1  =  9i>  2/2  =  92-  Using 
the  above  equations  and  expressing  the  constraint  equations  in  a  matrix  form,  we  obtain 


ICi  +  \C\2 

0 

2/i 

0 

-ISi  -  fs12 

l  c 

"2*12  J 

.  2/2 

—  V 

differentiating  equations  (??)(??)  once  more,  we  get  the  constraints  at  the  acceleration  level 
as 


h\  =  (/Ci  —  -Ci2)j/i  +  ~C\2y2  —  (/Sij/i  -r  -5j2(i/i  -  1/2)  )  —  0  d) 

h2  =  (-ISi  -  l-Su)yi  -  l-Si2y2  -  {ICiyi  +  ^12(»i  -  2/2)2)  =  0  (72) 

which  can  be  written  in  matrix  form  as 


'  /C1  +  1C„ 

|u12 

2/1 

lS\y{  |5i2(yi  —  2/2)‘ 

—  IS\  -  ^5l2 

_  2^12  . 

2/2  _ 

/Cij/r  ^Ci2(i/i  +  j/2)2 

introducing  Baumgarte  method,  equation  (  73  )  takes  on  the  following  form 
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IC\  +  2^12  |Ci2  t/1 

~  2^12  ~^S\2  i/2 

ISiVi  +  5^12(3/1  +  Vi)2  —  ^[ICiyi  -r  l2C\2(yi  +  2/2)]  —  3(1  Si  •+■  ~Si2  —  |) 

It'll /{  -t*  2 ^12(2/1  t  2/2)’  —  Q[— ISiyi  —  2 S\2(yi  +  2/2)  +  vj  —  3(ICX  t  12  -r  vt  —  x0) 

(74) 

From  equation  (  70,73  or  74  )  the  Jacobian  constraint  matrix  is  seen  to  be 

B  =  lCl  +  2°U  2°12  (75) 

.  -ISx  -  ±Sn  ~l2S12 

From  Kane  s  equation,  i.e.  F*  -f  F  +  BT A  =  0  ,  we  write  the  equations  of  motion  for  the 
system  as 


| ml"  —  I\  -r  I2  \rri^1C2  ■+■  I2 
\ml2C2  t  h  hml 2  ~  U 


—  \rnl2y2(y\  4-  y2)S2  -  2mglSx 
\ml2y\S2  -r  \mglSu 


lCl  +  -ISi  -  l-Su  1  A: 

2^12  —  W 

1  he  governing  equations  of  motion  for  the  two  bar  system  are  given  by  equation  (  73  and 
76  )  for  the  case  when  Baumgarte  method  is  not  used  and  by  equation  (  74  and  76  )  with 
Baumgarte  representation  of  the  constraints. 

We  can  easily  deduce,  from  equation  (  73  and  74  ),  when  singularity  occurs,  i.e.  qi  = 
g2  —  ",  the  first  row  of  B  vanishes.  Employing  the  method  proposed,  the  new  representation 
of  the  constraints  is  given  below. 


—3lSiyi  -  3J  Su(yi  -r  2/2)  ~jSX2(yi  -r  y2) 
—  ISl  ~  nSX2  ~iSl2 


ICiy3  "  ^’12(2/1  ~  >/2 )3 

1  ^  l  t  \  2 

^  1  if i  i2\ ji  y 2) 


Introducing  Baumgarte's  method,  equation  (  77  )  becomes 
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-3/Sii/i  —  jSi2(yi  -i-  2/2)  -f5i2(2/i+2/2) 

2/1 

—ISi  —  |5i2  —  |Si2 

3/2 

i  +  2 ^12(2/1  +  3/2 )3  -  &(^Ci2/i  +  \C\2(y\  +  3/2 )]  —  0(lSi  +  ^Si2  — 

_  ICiyi  +  {Cuiyi  -r  3/2 )2  -  a[--35ij/i  -  {Sn(yi  +  2/2)  +  t’j  -  &(lCi  -f  \C\2  +■  vt  -  xQ) 

Assume  the  two-bar  links  are  slender  rods,  and 


mi  —  2  /eg,  m2  =  1  /eg,  l  =  1  m, 

/1  =  —mil2  =  0.167  /eg  —  m:  /2  =  —  m2(-)2  =  0.0208  kg  —  m 2 

the  initial  angular  positions  for  link  1  and  link  2  are,  respectively,  1.047198  rad  and  4.41466 
rad.  while  the  initial  angular  velocities  are  given  to  be  1.4252  rad/s  and  -3.51724  rad/s. 

As  we  approach  singularity  we  should  experience  the  necessary  jumps  if  one  proceed  with 
the  standard  technique  where  the  constraints  are  not  eliminated.  To  test  this  problem  we 
have  run  a  simulation  and  found  some  very  interesting  results.  The  cases  are  labelled  as 
with  and  without  the  modified  B.  The  first  one  being  the  B  at  hand  and  its  corresponding 
constraint  forces  and  the  second  one  corresponds  to  the  proposed  stability  method  with 
the  modification  of  B.  In  .addition,  we  have  run  our  simulation  for  the  previous  two  cases 
with  and  without  Baumgarte  technique  to  further  see  whether  there  were  any  gams  in  the 
stability  procedure.  Figure  2  and  figure  3.  display  the  constraint  forces  at  the  neighborhood 
of  singularity.  It  is  very  clear  that  the  stability  method  proposed  (  solid  line  )  assures 
smoothness  of  motion.  The  constraint  forces  do  not  experience  any  jumps  as  is  the  case 
when  B  is  kept  unchanged. 

This  is  further  illustrated  by  figure  4.  where  the  jumps  in  velocity  is  seen  to  yield 
large  peaks.  The  latter  could  hamper  the  system  performance.  In  figure  5-6-7  the  same 
simulation  is  repeated  introducing  Baumgarte  technique.  We  can  easily  conclude  that  at 
singular  position  Baumgarte  technique  doesn't  provide  any  stability.  This  is  quite  excepted 
since  the  B  matrix  if  kept  unchanged  the  vanishing  of  the  constraint  will  cause  the  jumps 
we  are  seeing.  The  results  also  show  that  the  stability  provided  by  the  method  in  this  paper 
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is  not  altered  when  adding  Baumgarte  technique  to  it.  We  believe  that  the  combination  of 
Baumgarte  technique  and  the  proposed  method  herein  could  serve  as  a  unique  feature  in 
conducting  simulations  of  articulated  mechanical  systems. 


4.2  Discussion 

In  this  investigation  we  introduced  a  stability  method  needed  to  regulate  the  motion 
when  we  operate  in  the  presence  of  singularity.  The  constraint  forces  (driving  forces)  are 
shown  that  they  can  be  kept  smooth  and  continuous  by  using  the  proposed  technique  in 
handling  the  constraints  at  singular  positions.  Detecting  and  modifying  their  corresponding 
constraint  equations  could  be  done  during  the  process  of  the  evaluation  of  the  orthogonal 
complement  array  to  B.  Baumgarte  technique  is  seen  that  it  can  be  extended  to  incorporate 
stability  method  proposed  to  further  provide  a  more  robust  control  forces  in  the  dynamics 
of  constrained  multibody  systems.  We  envisage  some  important  finding  if  this  method  is 
applied  to  flexible  multibody  systems. 

5.  Stability  and  Control  of  Large  Scale  Flexible  Articulated  Sys¬ 
tems 

5.1  Equations  of  motion  based  on  recursive  formulation 

The  inter-connection  between  two  flexible  bodies  B *.  and  B:  is  shown  in  Figure  8.  Xk 
and  N3  are  the  floating  reference  frames  for  Bk  and  B}  located  outside  the  bodies,  with 
respect  to  which  the  associated  elastic  deformations  are  evaluated.  These  are  transformed 
to  the  inertial  reference  frame  R.  through  the  local  reference  frames  nk  and  n3  for  the  two 
bodies  located  at  Q *.  and  Qj.  The  rigid  body  rotations/translations  of  the  bodies  are  with 
reference  to  nk  .  The  bodies  could  be  discretized  using  suitable  finite  elements,  based  on  the 
geometric  configuration,  each  element  i  having  its  local  reference  frame  nk' .  Further  details 
of  the  relative  and  floating  reference  frames  and  the  the  different  associated  vectors  has 
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been  explained  in  the  reference  ill].  Concepts  of  the  indices  of  reference  arrays  have  been 
incorporated  in  the  kinematical  equations,  which  were  shown  to  improve  the  computational 
efficiency. 

The  mathematicl  equations  describing  the  vectors  q  and  d.  which  describe  the  position 
of  a  particular  point  on  the  body  and  the  body  vector  respectively,  and  the  angular  velocity 
Q,  could  be  written  in  terms  of  the  shape  function  matrices  denoted  as  J\f  and  M.  (  for  the 
linear  and  rotational  elastic  deformation  ),  which  includes  the  necessary  transformations  as 


qkl  =  M'k'uki  -  Mkuk 

(79) 

dk  =  Krk'uk'  -  M]ui 

(80) 

;n“  =  Mk'dJr  -  M]u3r 

(81) 

nknNk  =  -Mkuc 

(82) 

where  ur  represents  the  identified  and  extracted  vector,  containing  the  exact  kinematical 
quantities  associated  either  with  the  element  or  the  flexible  body. 

The  generalized  speeds  y  are  written  as  : 


y  = 


Qr.Tr.nr 


(83) 


where  Q.  7,  q  are  the  vectors  of  the  components  of  the  angular  velocity  of  the  bodies  in 
the  system,  derivatives  of  the  components  of  translational  velocities  and  the  derivatives  of 
the  components  of  modal  co-ordinates  of  the  flexible  bodies  in  the  system,  respectively.  The 
relationship  between  the  derivatives  of  Euler  angles  0t  or  the  Euler  parameters  0,  are  given 
bv  : 


ek  =  Dnk 

(84) 

ek  =  £nk 

(85) 

where  D  and  E  are  the  related  transformation  matrices. 

The  use  of  component  mode  sysnthesis  lies  in  expressing  rotational  elastic  deformation 
and  translational  elastic  deformation  f.  in  terms  of  nodal-modal-tranformation  matrix  \ 


and  the  modal  co-ordinate  vectors  77,  associated  with  the  selected  mode  shapes  as  : 

=  M*x*ri?  (86) 

fki  =Afkix*:r,?  (87) 

where  the  subscript  e  refers  to  the  index  of  reference  array  edof  [  12 

A  more  detailed  explanation  of  the  kinematical  matrix  representation  could  be  seen  in 
the  reference  I  12  ].  Let  us  now  discuss  in  brief,  the  kinematical  equations  involved  herein. 

The  partial  velocity  vectors  s  and  £  associated  with  the  rigid  and  flexible  motions  respec¬ 
tively.  could  be  utilized  to  express  the  angular  velocities  of  any  body  k  of  the  multi-body 
system  in  R  as  : 

Qk  =  -  tkT]  (88  ) 


Clk'  = 


where 


^ 


?  =  ?  -  SkMkU  1 91) 

Once  more  k  and  k *  refers  to  the  unit  vectors,  fixed  at  the  inter-connected  elements  of  the 
adjacent  bodies  B *.  and  B:  (  see  Figure  8  ).  The  subscripts  eb  refers  to  t he  indices  connected 
with  both  the  reference  arrays  edof  and  bdof  12  .  The  velocity  and  acceleration  of  an 
arbitrary  point,  in  a  similar  fashion  could  be  written  as  : 

Vki  =  7fcin  -r  i/fct  -r  Jk'Ti  (92) 


Ak'  =  7*'fi  -e  „*T  +  3ktij  -  +  i>fcT  *  3k'n  (93) 

where  7,  v  and  3  are  again  the  partial  velocity  arrays,  associated  with  Q.  T  and  r)  respectively. 
u  is  same  as  the  purtitil  angular  velocity  matrix.  7  and  3  could  be  written  in  a  recursively 
computed  form  as: 

7*=7*^I/  (94) 
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3k'  =  3k  4-  [  sk  [  .\fk'xkr'b  -  .Vkxtb  ]  -  <ik'ZkX  ]  (95) 

where 

~)k  =  7J  +  Yfci 3  +  d*  v:  (96) 

and 

/  -  -  [  Tk(j  -  &(>  ]  \  +  S3Ark\kc;  -  S’ A r>xib  (97) 

It  is  worth  mentioning  at  this  point  the  recursive  nature  of  the  computation  of  kinematical 
quantities  as  indicated  by  the  superscripts  j  and  k  for  the  lower  and  upper  bodies  respectively. 
The  process  of  computation  starts  at  the  lowest  body  level,  where  all  the  quantities  are  known 
and  through  subsequent  recursive  substitution  into  the  expressions,  the  quantities  required 
ultimately  for  the  point  under  consideration  are  evaluated. 

The  details  of  the  equations  of  motion  in  a  matrix  representation  form  based  on  Kane  s 
equations  .  is  given  in  reference  12  which  after  simplification  yields  : 

My~V~Q~\J  =  f  198) 


where  At.  O,  A.  J .  T  and  V  represent  the  co-efficients  matrices  for  the  eeneralized  co¬ 
ordinates  accelerations,  quadratic  velocity  vector,  the  c  dimensional  vector  of  undetermined 
multipliers,  constraint  jacobian  matrix,  generalized  external  forces  and  stiffness  due  to  strain 
energy  ot  the  flexible  bodies  m  the  system  respectively.  These  matrices  are  civen  by  At  = 

'L'L.J  (HkJ'k,dV  OydV  ZZJ  PlhJ>'k'W.3yjV  ZZj  ,nk,TJk,W  oyjv 
Z Z I p‘:k,Ti'k,m '  oyj\ ’  ZZJ />»/*’ Vv-n '  oyj\ '  ZZJ pi'k,T-ik,w ’  -0’«a •  j  1 99 1 


[  ZZSnklTsktdv  nyjv  zZJ/^k,T^\':<>ydv  ZZJpJkJ Jkiw  <‘W 

where  />  is  the  mass  density  of  the  material  used.  ^  is  the  summation  < .f  the  volume 
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the  element  \  over  all  the  elements  of  a  body  and  all  the  bodies  in  the  system. 
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kt  ...kiT 


V"  T'  I'Kl 

L.  L Jr  Pu 


ZESvP^k 


jkt  Q  +  r>fcf  -  3ktn 


jklQ  -r  r*T  -r 


7fc,n  -i-  i>*f  - 
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T  = 


Jsk.  ~fk'TfktdS  -  f  yk'J  bk'd\ 


[■J, 


^JSk,l'k,Ifk'dS~fisk'Tbk'd\ 


ZZfsk  3k'T fkldS  -  f3k'Tbk'dY 
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where  Z*11  and  6fcl  are  the  surface  traction  and  body  forces  associated  with  the  global  boundary 
of  the  finite  elements  and  their  weights  respectively, 
and 


Z7  =  ;  l»  ; 

i  1 

\r  :  K  -  G  }  \p  \ 

L  J 

where  A  and  G  are  t lie  block  diagonal  matrices,  whose  diagonal  sub-matrices  are  A 
(7  .  being  refered  to  the  structural  stillness  and  geometric  stiffness  for  bodv  A 


5.2  Derivation  of  the  Constraint  equations  : 

1  se  of  partial  velocity  arrays  m  the  recursive  formulation  of  the  the  dynamical  equa¬ 
tions  of  motion,  renders  a  noteworthy  benefit  of  the  automatic  generation  of  the  constraint 
equations.  Basically,  the  various  constraints  could  be  broadlv  classified  as: 

1  Closed  loops 
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2.  Prescribed  motions 

3.  Contact  between  inter-connected  bodies. 

Closed  loops  are  a  common  type  of  constraint,  found  in  various  mechanisms.  The  joining 
point,  where  the  closed  loop  occurs,  could  be  reached  in  two  ways,  from  the  fixed  inertial 
reference  frame.  Denoting  these  two  ways  by  p  and  q,  the  constraint  equation  could  be 
written  in  a  compact  form  as  : 


(103) 


The  specification  of  the  kinematical  quantities  such  as  speeds  and  angular  velocities  form 
the  second  prescribed  motions.  T'ie  later  are  based  on  the  linear  and  angular  velocity,  as  a 
function  of  time,  say,  G(t)  and  h(t),  where  the  constraint  equation  take  the  following  form  : 
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3T 


n 

t 

v 


=  G(t) 


(104) 


r  _ 

Q 

V 

(105) 


where  r  refers  to  the  position  vector  C,T  of  a  point  .4.  which  has  the  specified  velocity,  in 
local  reference  frame  of  body  A’. 

In  a  similar  fashion,  the  third  type  of  constraint  mentioned  above,  is  defined  by  the 
number  of  points  used  to  describe  the  contact  between  a  gear/ shaft  and  a  gear  gear  type  of 
bodies.  It  will  be  shown  in  the  following  sections  of  the  paper,  that  the  jacobian  matrices 
associated  with  the  generalized  co-ordinate  derivatives,  could  be  generated  automatically, 
for  the  cases  of  both  the  rigid  and  flexible  gears/shafts. 

The  Holonomic  and  Non-holonomic  constraints  at  the  velocity  level  could  be  written  in 
a  compact  form  as  : 


jy  =  G(t) 


(106) 
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where,  J  is  the  jacobian  constraint  matrix  of  order  c  by  n,  c  being  the  number  of  the 
constraints  in  the  system  and  n,  the  number  of  generalized  co-ordinates. 

Differentiating  the  above  equation  once  more  and  combining  it  with  equation  (  98  )  after 
the  elimination  of  the  undetermined  multipliers,  we  get 

cy  =  n  (107) 

where 

C=[ctm\  (108) 

and 

U  =  CT  jr-p-Q  (109) 

C  in  the  above  equation  is  the  orthogonal-complement  to  the  jacobian  matrix  J ,  and  is 
obtained  by  Psuedo-uptriangular  Decomposition  method.  Subsequent  numerical  integration 
of  the  governing  equations  of  motion  (  107  ),  would  yield  the  time  history  of  the  generalized 
speeds  or  generalized  co-ordinates. 

5.3  Vectorization  and  its  computer  implementation 

The  above  developed  algorithmic  procedure  is  implemented  on  the  various  supercomput¬ 
ers  and  mainframes. 

The  code  by  its  nature  of  dynamical  equations  derived  on  the  basis  of  flexibility  and 
finite  element  approach,  constitute  a  substantial  part  of  long  vectors /arrays.  This  feature 
makes  it  a  potential  candidate  for  the  exploitation  of  the  pipelining  technique  of  modern 
Vector-Processor  hardware  in  supercomputers.  This  technique  allows  the  overlapping  of 
instructions  to  a  sequence  of  operands  simultaneously,  as  compared  to  a  single  operand  in  a 
scalar  processor.  Thus,  speedup  is  achieved  by  a  kind  of  micro-parallelism,  in  which,  different 
stages  of  pipeline  work  simultaneously  cn  different  data.  In  general,  performance  increases 
with  the  vector  length  upto  a  limit  governed  by  the  section  size  of  the  Vector- Pipe.  The 


section  size  refers  to  the  number  of  elements  a  Vector- Pipe  could  hold  at  a  particular  point 
of  time.  (  The  section  size  varies  with  the  machine,  like  it  is  256  in  IBM-3090  and  64  in  the 
CRAY  ). 

Effort  was  made  to  improve  the  efficiency  of  execution  of  the  implemented  code  by  reduc¬ 
ing  the  CPU  time,  by  vectorizing  the  code.  This  was  carried  out  both  at  the  compiler  option 
level  (  outside  the  code  )  and  at  the  Do-loop  level  (  inside  the  code  )  in  the  computationally 
intensive  areas.  In  particular,  the  concepts  of  promoting  the  scalar  variables  to  vectors  in  the 
left  hand  side  of  the  expressions,  was  found  to  be  more  effective  in  certain  computationally 
intensive  areas,  which  improved  the  overall  speed  of  execution  of  the  code. 


5.4  Proposed  Stability  and  Control  for  Systems  Operating  in  the  Presence  of 
Singularity 

In  our  future  work,  we  intend  to  extend  the  developments  of  phase  1  of  this  project  for 
large  class  of  multibody  systems  including  those  with  the  elastic  bodies.  The  validity  of 
the  developed  method  presented  in  phase  1  will  give  us  a  new  insight  on  the  basic  control 
research  problems  for  articulated  structures  undergoing  quick  maneuvers  and  operating  in 
the  presence  of  singularities. 


6  Conclusions  and  Future  Directions 

We  have  demonstrated  under  the  proposed  contract  how  the  PUTD  could  be  extended 
to  handle  singularities  in  the  dynamics  of  articulated  structures.  A  stability  method  was  de¬ 
veloped  to  regularize  the  vanishing  and  linearly  dependent  constraints  that  are  time  variant, 
through  a  new  representation  making  use  of  higher  order  derivatives.  The  continuity  of  the 
motion  is  demonstrated  through  an  example  of  two  arm  robot  driven  through  singularity. 
Our  goal  is  to  extend  this  development  and  results  to  realistic  models  of  structures  with 
flexible  bodies. 
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The  second  phase  has  been  initiated  where  an  all  purpose  code  was  developed  and  subject 
to  vectorization  in  supercomputers  to  test  its  ability  to  achieve  real  time  simulations.  We 
believe  that  our  efforts,  if  continued  will  lead  to  some  major  contributions  in  the  control  and 
stability  of  constrained  multibody  systems.  In  particular  quick  maneuvers  of  space  structures 
and  basic  ground  research  of  robotics  could  benefit  greatly  from  the  implementation  of  these 
algorithms. 
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Figure  &.  Labelling  position  vectors  in  adjacent  bodies. 
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